﻿#include "gait_math.h"
#include "eso.h"
#include "locomotion_header.h"
#include "include.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

Gait_Bound_IMP_Param ftrot_slip_param;
Gait_Bound_IMP ftrot_slip[2];
Gait_Bound_IMP ftrot_slip_leg[4];
Gait_Bound_IMP ftrot_slip_all;


void ftrot_gait_init(void)
{
	int i;
	ftrot_slip_param.first_bound = 0;
	ftrot_slip_param.mess = Mw;
	ftrot_slip_param.c = 0.7;
	ftrot_slip_param.exp_z = 0.76*fabs(MAX_Z);

	ftrot_slip_param.state_all = 0;
	ftrot_slip_param.L = 0.57*Hw;
	ftrot_slip_param.vd = 0.5;
	ftrot_slip_param.tauP = 20;//Nm/rad

	ftrot_slip_param.k_time = 0.25;
	ftrot_slip_param.s_peak_Fz = 0.45;

	ftrot_slip_param.T_sw = vmc_all.param.param_vmc_default.stance_time[0];
	ftrot_slip_param.T_air = 0.025;
	ftrot_slip_param.T_st_norm = ftrot_slip_param.T_sw -2* ftrot_slip_param.T_air;
	for (i = 0; i < 2; i++) {
		ftrot_slip[i].wait_trig = 0;

		ftrot_slip[i].T_st_now = ftrot_slip[i].T_st_last = ftrot_slip_param.T_st_norm;

		ftrot_slip[i].T_sw = ftrot_slip_param.T_sw;
		ftrot_slip[i].T = ftrot_slip_param.T_sw + ftrot_slip[i].T_st_now;

		ftrot_slip[i].t = ftrot_slip[i].s = 0;
		ftrot_slip[i].time_fly_near = 0;
		ftrot_slip[i].g_st = 0;
		ftrot_slip[i].ground = 0;

		ftrot_slip[i].hip_n_exp.z = ftrot_slip_param.exp_z;

		ftrot_slip[i].pid_hip[0] = 0;// 800;//高度
		ftrot_slip[i].pid_hip[2] = 0;

		ftrot_slip[i].pid_v[0] = 0;// 350;//速度
		ftrot_slip[i].pid_v[1] = 0;//速度

		ftrot_slip[i].pid_P[0] = 0;// 8;//俯仰角
		ftrot_slip[i].pid_P[2] = 0;

		ftrot_slip[i].k_jump = 1;//Fz前馈比例

		vmc_robot_p.ground_mu = 0.5;

		ftrot_slip[i].g_st = 1;

		ftrot_slip[i].T_air = (ftrot_slip[i].T_sw - ftrot_slip[i].T_st_now) / 2;

		ftrot_slip[i].T_st_now_trig = ftrot_slip[i].T_st_now;//触发式
		ftrot_slip[i].T_tirg = ftrot_slip[i].T;
	}
	ftrot_slip[F].phase = F;
	ftrot_slip[B].phase = B;
	ftrot_slip[F].other_phase = B;
	ftrot_slip[B].other_phase = F;

	ftrot_slip[F].ground = 1;//后腿先跳
	ftrot_slip[B].ground = 0;

	ftrot_slip_param.sw_dh = 0.025;

	ftrot_slip_param.first_bound = 0;
#if RUN_WEBOTS
	printf("IMP Ftort Initing!!!!!!!\n");
#endif
}

float cal_BezierP3f(float s, float a, float b, float c, float d)//三阶贝塞尔
{
	float temp = 1 - limitw(s, 0, 1);
	float o_temp = a * temp* temp * temp + 3 * b * s * temp * temp + 3 * c * s * s * temp + d * s * s * s;
	return o_temp;
}

float cal_Impluse_Fzf(float alfa, float s_st, float s_peak)//Z方力前馈
{
	float fz;
	float s1, s2;

	if (s_st <= s_peak) {
		s1 = s_st / s_peak;
		fz = alfa * cal_BezierP3f(s1, 0, 0.8, 1, 1);
	}
	else {
		s2 = (s_st - s_peak) / (1 - s_peak);
		fz = alfa * cal_BezierP3f(s2, 1, 1, 0.8, 0);

	}
	return fz;
}

float cal_Impluse_gsf(float s_st)//gs
{
	float s_peak = 0.5;
	float s1, s2;
	float gs;
	if (s_st <= s_peak) {
		s1 = s_st / s_peak;
		gs = cal_BezierP3f(s1, 0, 1, 1, 1);
	}
	else {
		s2 = (s_st - s_peak) / (1 - s_peak);
		gs = cal_BezierP3f(s2, 1, 1, 1, 0);

	}
	return gs;
}

float cal_Impluse_tauPf(float alfa, float s_st, char sel)//俯仰力矩前馈
{
	float tauP;
	float s_peak = 0.5;
	float s1, s2;

	if (sel == F) {
		if (s_st <= s_peak) {
			s1 = s_st / s_peak;
			tauP = -alfa * cal_BezierP3f(s1, 0, 0.8, 1, 1);
		}
		else {
			s2 = (s_st - s_peak) / (1 - s_peak);
			tauP = -alfa * cal_BezierP3f(s2, 1, 1, 0.8, 0);
		}
	}
	else {
		if (s_st <= s_peak) {
			s1 = s_st / s_peak;
			tauP = alfa * cal_BezierP3f(s1, 0, 0.8, 1, 1);
		}
		else {
			s2 = (s_st - s_peak) / (1 - s_peak);
			tauP = alfa * cal_BezierP3f(s2, 1, 1, 0.8, 0);
		}
	}
	return tauP;
}
 
void sensor_process(float dt)
{
	int i;
	for (i = 0; i < 2; i++) {
		ftrot_slip[i].att_now[PITr] = vmc_all.att[PITr];
		ftrot_slip[i].att_now_rate[PITr] = vmc_all.att_rate[PITr];
		ftrot_slip[i].att_exp[PITr] = -vmc_all.ground_att_cmd[PITr];
		//printf("vmc_all.ground_att_cmd[PITr]=%f\n", vmc_all.ground_att_cmd[PITr]);
	}

	ftrot_slip[F].hip_n_now.x = (vmc[0].epos_n.x + vmc[3].epos_n.x) / 2;
	ftrot_slip[F].hip_n_now.z = (vmc[0].epos_n.z + vmc[3].epos_n.z) / 2;

	ftrot_slip[B].hip_n_now.x = (vmc[1].epos_n.x + vmc[2].epos_n.x) / 2;
	ftrot_slip[B].hip_n_now.z = (vmc[1].epos_n.z + vmc[2].epos_n.z) / 2;

	ftrot_slip[F].hip_nb_now.x = (vmc[0].epos_n_b.x + vmc[3].epos_n_b.x) / 2;
	ftrot_slip[F].hip_nb_now.z = (vmc[0].epos_n_b.z + vmc[3].epos_n_b.z) / 2;

	ftrot_slip[B].hip_nb_now.x = (vmc[1].epos_n_b.x + vmc[2].epos_n_b.x) / 2;
	ftrot_slip[B].hip_nb_now.z = (vmc[1].epos_n_b.z + vmc[2].epos_n_b.z) / 2;

	ftrot_slip[F].dhip_n_now.x = (vmc[0].spd_n.x + vmc[3].spd_n.x) / 2;
	ftrot_slip[F].dhip_n_now.z = (vmc[0].spd_n.z + vmc[3].spd_n.z) / 2;

	ftrot_slip[B].dhip_n_now.x = (vmc[1].spd_n.x + vmc[2].spd_n.x) / 2;
	ftrot_slip[B].dhip_n_now.z = (vmc[1].spd_n.z + vmc[2].spd_n.z) / 2;

	ftrot_slip[F].touch = vmc[0].is_touch && vmc[3].is_touch;
	ftrot_slip[B].touch = vmc[1].is_touch && vmc[2].is_touch;

	//RC_CMD
	//ftrot_slip_param.vd = 0.1;//
	//tar_air_z = -0.65*fabs(MAX_Z);

	for (i = 0; i < 2; i++) {
		ftrot_slip[F].dhip_n_exp.x = ftrot_slip_param.vd ;
		ftrot_slip[B].dhip_n_exp.x = ftrot_slip_param.vd ;
		ftrot_slip[F].hip_n_exp.z = ftrot_slip_param.exp_z  ;
		ftrot_slip[B].hip_n_exp.z = ftrot_slip_param.exp_z  ;
	}
}

#define EN_TIME_CAL_ALAWYS1 0
#define F_CONTROL_ALAWYS1   0//不如2腿好奔跑时候

void force_implusef(Gait_Bound_IMP *slip, float dt)//计算前馈脉冲
{
	slip->alfa_Fz = ftrot_slip_param.mess*9.81*slip->T / (2 * ftrot_slip_param.c*slip->T_st_now);
	slip->alfa_tauP = ftrot_slip_param.tauP;//速度 支撑时间相关

	slip->s = slip->t / slip->T_st_now;
	slip->s = limitw(slip->s, 0, 1);

	slip->Fz_imp = cal_Impluse_Fzf(slip->alfa_Fz, slip->s, ftrot_slip_param.s_peak_Fz)*slip->k_jump;
	//slip->TauP_imp = cal_Impluse_tauPf(slip->alfa_tauP, slip->s, slip->phase);
	slip->g_st = cal_Impluse_gsf(slip->s);

	if (slip->phase == F && 0)
		printf("%f\n", slip->g_st);
}

void gait_pattern_controlf(Gait_Bound_IMP *slip)//支撑时间修正
{
	if (gait_bound.slip_param.vd == 0 || 1)
		slip->T_st_norm = 0.35;
	else if (fabs(gait_bound.slip_param.vd > 0.01))
		slip->T_st_norm = 0.133 + 0.4 / gait_bound.slip_param.vd*(0.16 - 0.133);


	slip->T_st_norm = limitw(slip->T_st_norm, 0.1, 0.35);// +jump_off_st[slip->phase];
	printf("T_st_norm_spd=%f\n", slip->T_st_norm);

	slip->T_air = (slip->time_fly_near - slip->T_st_last) / 2;

	slip->T_st_now = slip->T_st_norm - ftrot_slip_param.k_time*(slip->T_st_last + slip->T_air - slip->T / 2);
	slip->T_st_now = limitw(slip->T_st_now, 0.1, 0.5);

	//slip->T_st_now = slip->T_st_norm;//不修正

	slip->T_st_last = slip->T_st_now;

	slip->T = slip->T_sw + slip->T_st_now;

	slip->T_st_now_trig = slip->T_st_now;
	slip->T_tirg = slip->T;
}

void balance_controlf(Gait_Bound_IMP *slip, float dt)//支撑平衡控制
{
	float Fx_ff = 0;
	float Fz_ff = 0;
	Fx_ff = 0;// 1 / slip->hip_n_now.z*(slip->TauP_imp * 1 + slip->hip_n_now.x*slip->Fz_imp * 1);//IMP前馈
	Fz_ff = slip->Fz_imp;

	float Fz_hip = 0;//高度
	Fz_hip = slip->g_st*(slip->pid_hip[0] * (slip->hip_n_exp.z - fabs(slip->hip_nb_now.z)) - slip->pid_hip[2] * slip->dhip_n_now.z);
	if (slip->phase == B && 0)
		printf("kp=%f exp_z=%f now_z=%f Fz_hip=%f\n", slip->pid_hip[0], slip->hip_n_exp.z, slip->hip_nb_now.z, Fz_hip);
	float Fx_v = 0;//速度
	Fx_v = slip->pid_v[0] * (slip->dhip_n_exp.x - slip->dhip_n_now.x) ;//FF力

	if (slip->phase == F && 0)
		printf("kp=%f exp_v=%f now_v=%f\n", slip->pid_v[0], slip->dhip_n_exp.x, slip->dhip_n_now.x);
	float Fz_p = 0;//姿态
	Fz_p = -1 / slip->hip_n_now.x*(slip->pid_P[0] * (slip->att_now[PITr] - slip->att_exp[PITr]) + slip->pid_P[2] * slip->att_now_rate[PITr]) / 57.3;

	slip->Fx = Fx_ff + Fx_v;
 
	slip->Fz = Fz_ff + Fz_hip + Fz_p;

	if (slip->phase == F && 0)
		printf("Fz_ff=%f Fz_hip=%f Fz_p=%f Fx_v=%f Tst=%f\n", Fz_ff, Fz_hip, Fz_p, Fx_v, slip->T_st_now);
}

void slip_state_machinef(Gait_Bound_IMP *slip, float dt)//独立状态机
{
	static int en_other = 0;
	static float t_en_other = 0;
	static float delay_to_jump = 0;
	static char jump_state = 0;
	static char g_reg[2] = { 0 };
	static float time_fly = 0;
	static float t_init = 0;
	char trig_state_flag[2] = { 0,0 };
	char id_next = 0;
	slip->Fx = slip->Fz = slip->Fz_imp = slip->TauP_imp = 0;
	slip->g_st = 1;
 

	if (ftrot_slip_param.first_bound == 1) {//另一条腿首次启动
		t_init += dt;

		if (t_init > slip->T_air||1) {
			ftrot_slip[slip->phase].ground = 1;
			ftrot_slip_param.first_bound = 2;
#if RUN_WEBOTS
			printf("Other Phase Active!!!!!!!\n");
#endif
		}
	}
#if F_CONTROL_ALAWYS1
	if (slip->ground && 1) {
		force_implusef(slip, dt);
		balance_controlf(slip, dt);
	}
#endif

	switch (slip->state)
	{
	case 0:
		slip->t_start += dt;
		if (slip->ground
			&& (
				ftrot_slip_param.first_bound != 2
				||1
				//|| slip->t_start> slip->T_air*2
				//|| slip->t_start > (slip->T_air  - slip->T_st_last)
				//|| (ftrot_slip[slip->other_phase].ground&&slip->t_start > slip->T*0.5) //非同时着地约束 
				)
			)
		{
			slip->t = slip->t_start = 0;
			slip->state++;
#if !EN_TIME_CAL_ALAWYS
			slip->T_st_now = slip->T_st_now_trig;//触发式
			//slip->T = slip->T_tirg;
#endif 
		}
		break;
	case 1://支撑+离地检测
		slip->t += dt;
#if !F_CONTROL_ALAWYS1
		force_implusef(slip, dt);
		balance_controlf(slip, dt);
#endif
		//if (slip->t >= slip->T_st_now) {
		//	if (slip->wait_trig==0
		//		&&ftrot_slip[slip->other_phase].state == 1
		//		&&ftrot_slip[slip->other_phase].t >= ftrot_slip[slip->other_phase].T_st_now
		//		&&ftrot_slip[slip->other_phase].wait_trig == 0) {//相序差
		//		slip->wait_trig = 1;
		//		slip->wait_trig_time = 0;
		//	}
		//	if (slip->wait_trig == 1 && ftrot_slip[slip->other_phase].state >= 2)
		//		slip->wait_trig = 2;

		//	if(slip->wait_trig==2)
		//		slip->wait_trig_time += dt;

		//	if(slip->wait_trig == 2&&slip->wait_trig_time> slip->T_sw)
		//		slip->wait_trig = 0;
		//}

		
		if (((slip->t >= slip->T_st_now && 1)&&
			(slip->wait_trig==0))//防止四腿支撑
			//|| (!slip->touch &&!ftrot_slip[slip->other_phase].touch&&slip->t > slip->T_st_now *0.55)//同时四脚离地  仿真里会导致 后腿软吊
			//|| (!slip->touch&&slip->t > slip->T_st_now *0.5)//同时四脚离地
			//|| (!slip->touch&&slip->t > slip->T_st_now *0.25)//同时四脚离地
			//||(slip->dhip_n_now.z<0.9*MAX_Z&&slip->t> slip->T_st_now/2)
			) {//-------------------------开始摆动
			if(ftrot_slip_param.first_bound==0)
				ftrot_slip_param.first_bound = 1;//另一条腿开始支撑

			slip->t = slip->time_fly_near = 0;//记录飞行时间
			slip->ground = 0;
			slip->state++;

			if (slip->phase == F) {//切换摆动参数
				slip->id_sw[0] = 0; slip->id_sw[1] = 3;
			}
			else {
				slip->id_sw[0] = 1; slip->id_sw[1] = 2;
			}

			trig_plan(slip->id_sw[0], dt);
			trig_plan(slip->id_sw[1], dt);
			vmc[slip->id_sw[0]].param.trig_state = vmc[slip->id_sw[1]].param.trig_state = 1;
			//printf("phase=%d LF\n", slip->phase); 
		}
		break;
	case 2://lift
		//if (ftrot_slip[slip->other_phase].ground == 0)
			slip->time_fly_near += dt;

		trig_state_flag[slip->id_sw[0]] = trig_lift(slip->id_sw[0], dt);//ground->0
		trig_state_flag[slip->id_sw[1]] = trig_lift(slip->id_sw[1], dt);
		if (trig_state_flag[slip->id_sw[0]] && trig_state_flag[slip->id_sw[1]]) {
			vmc[slip->id_sw[0]].param.trig_state = 2;
			vmc[slip->id_sw[1]].param.trig_state = 2;
			slip->state++;
		}
	break;
	case 3: //摆动+TD
		//if (ftrot_slip[slip->other_phase].ground == 0)
			slip->time_fly_near += dt;

		for (int i = 0; i < 2; i++) {
			switch (vmc[slip->id_sw[i]].param.trig_state)
			{
			case 2:
				trig_state_flag[slip->id_sw[i]] = trig_swing(slip->id_sw[i], dt);
				if (trig_state_flag[slip->id_sw[i]] == 1)//�л�֧��
					vmc[slip->id_sw[i]].param.trig_state = 3;
				if (trig_state_flag[slip->id_sw[i]] == 2)//�л�TD
					vmc[slip->id_sw[i]].param.trig_state = 4;
				break;
			case 3:
				trig_state_flag[slip->id_sw[i]] = trig_td(slip->id_sw[i], dt);
				if (trig_state_flag[slip->id_sw[i]] >= 1)//�л�֧��
					vmc[slip->id_sw[i]].param.trig_state = 4;
				break;
			}
		}

		//着地判断
		if ( 
			(//slip->touch
			 ((vmc[0].param.trig_state == 4 && vmc[3].param.trig_state == 4 && slip->phase == F)||
			  (vmc[1].param.trig_state == 4 && vmc[2].param.trig_state == 4 && slip->phase == B))
				 )//着地判断
			) {
			//printf("Phase=%d time_fly=%f\n",slip->phase, slip->time_fly_near);
			slip->ground = 1;
			slip->time_fly_near_last = slip->time_fly_near;//记录飞行时间

#if EN_TIME_CAL_ALAWYS1
			gait_pattern_controlf(slip);//修正时间	
#endif
			slip->state = 0;
			if (slip->phase == F) {

				vmc[0].ground = vmc[3].ground = 1;
				stand_force_enable_flag[0] = stand_force_enable_flag[3] = 1;
				vmc[0].param.trig_state = vmc[3].param.trig_state = 0;
			}
			else {

				vmc[1].ground = vmc[2].ground = 1;
				stand_force_enable_flag[1] = stand_force_enable_flag[2] = 1;
				vmc[1].param.trig_state = vmc[2].param.trig_state = 0;
			}

			//next leg
#if 0
			ftrot_slip_all.T_st_last = slip->T_st_now;
			ftrot_slip_all.time_fly_near_last = slip->time_fly_near_last;//最近的飞行时间
#if !EN_TIME_CAL_ALAWYS1
			gait_pattern_controlf(&ftrot_slip_all);
#endif 
			//next leg
			id_next = slip->other_phase;//下一条腿
			//slip->T_st_now_trig = ftrot_slip_all.T_st_now;
			ftrot_slip[id_next].T_st_now_trig = ftrot_slip_all.T_st_now;//触发更新支撑摆动时间
			//ftrot_slip[id_next].T = ftrot_slip[id_next].T_st_now_trig + ftrot_slip[id_next].T_sw;
			//vmc_all.stance_time= ftrot_slip[id_next].T_st_now_trig;
#else
			gait_bound.slip_all.T_st_last = slip->T_st_now;
			gait_bound.slip_all.time_fly_near = slip->time_fly_near;//最近的飞行时间
			gait_bound.slip_all.T = slip->T;
#if !EN_TIME_CAL_ALAWYS
			gait_pattern_controlf(&ftrot_slip_all);
#endif 
			//next leg
			id_next = slip->other_phase;//下一条腿
			ftrot_slip[id_next].T_st_now_trig = ftrot_slip_all.T_st_now;
			ftrot_slip[id_next].T_tirg = ftrot_slip[id_next].T_sw + ftrot_slip[id_next].T_st_now;

#endif
		}
		break;
	}
}

void  Gait_FTrot_Active(void)
{
	char i = 0;
	vmc_all.param.robot_mode = M_F_TROT;
	vmc_all.gait_mode = F_TROT;
	vmc_all.tar_att[YAWr] = vmc_all.att_ctrl[YAWr];
	vmc_all.tar_pos.z = vmc_all.pos_n.z;
	vmc_all.trot_state = 0;
	slip_mode.slip_F[Xr] = slip_mode.slip_F[Yr] = slip_mode.slip_F[Zr] = 0;
	slip_mode.slip_timer = 0;
	//vmc_robot_p.ground_mu = config_gait["trot_param"]["ground_mu"].as<float>();//0.35;
	robotwb.exp_att.yaw = robotwb.now_att.yaw;
	for (i = 0; i < 4; i++) {
		vmc[i].tar_sita1 = vmc[i].sita1;
		vmc[i].tar_sita2 = vmc[i].sita2;
		vmc[i].tar_sita3 = vmc[i].sita3;
		vmc[i].ground = 1;
		vmc_all.param.trot_sw_end_flag[i] = 0;
		vmc_all.param.gait_sw_end_reg_n[i] = vmc[i].epos_n;
		reset_tar_pos(i);
	}
	if (vmc_all.ground_num >= 2 && stand_force_enable_flag[4] && USE_FPOS_CONTROL && !TEST_TROT_SW) {//使能力控
		stand_force_enable_flag[0] = 1;
		stand_force_enable_flag[1] = 1;
		stand_force_enable_flag[2] = 1;
		stand_force_enable_flag[3] = 1;
		stand_force_enable_flag[4] = 1;
		vmc_all.trot_air_test = 0;
	}
	else
		vmc_all.trot_air_test = 1;

#if RUN_WEBOTS||RUN_PI
	if (vmc_all.trot_air_test)
		printf("FTort Gait Initing::Air Test!!!!!!!\n");
	else
		printf("FTort Gait Initing::Gait Mode!!!!!!!\n");
#endif
	ftrot_gait_init();
}

#if 1
void  Gait_FTrot_Update_v1(float dt){//old good
char trig_state_flag[4] = { 0,0,0,0 };
static char init[4], init_lisence = 0, state, cnt_time_change, switch_flag;
static float time_delay, yaw_force;
static float timer_task[10];
char i, sel;
float test_epos_b[3], test_epos_leg[3];
static END_POS end_pos_n[4], end_pos_n_use[4];//ĩ�˹̶���ȫ������ϵ�µ�λ��
static float cnt_mode_can_switch = 0, mode_en_switch_flag = 0;
static float att_test_use[3];
char leg_sel_trig = 0;
float att_off[2];
static float phast_time = 0;
static char leg_phase[2] = { 0 };
static char leg_other[2] = { 0 };
static char slip_greg[2] = { 0 };
char id_next = 0;
if (timer_task[2] > 0.02) {
	body_traj_planner(timer_task[2]);
	timer_task[2] = 0;
}


if (vmc_all.gait_mode == F_TROT)//----------------------�Խǲ�̬
{

	sensor_process(dt);//处理同步相序

	switch (vmc_all.trot_state)
	{
	case 0:
		if (cnt_time_change++ > 2 || 1) {//WS
		vmc_all.gait_time[1] = vmc_all.gait_time[3];
		vmc_all.stance_time = vmc_all.stance_time_auto;
		vmc_all.delay_time[2] = vmc_all.delay_time[1];
		cnt_time_change = 0;
		}

		vmc_all.trot_state++;
		slip_mode.slip_F[Xr] = slip_mode.slip_F[Yr] = slip_mode.slip_F[Zr] = 0;
		slip_mode.slip_timer = 0;
	break;
	case 1:
		slip_state_machinef(&ftrot_slip[F], dt);
		slip_state_machinef(&ftrot_slip[B], dt);

#if 0
		for (i = 0; i < 2; i++) {//--------------------时间相序控制
			if ((ftrot_slip[i].ground && !slip_greg[i])) {//impact last 
#if 0
					ftrot_slip_all.T_st_last = ftrot_slip[i].T_st_last;
					ftrot_slip_all.time_fly_near = ftrot_slip[i].time_fly_near_last;//最近的飞行时间
					ftrot_slip_all.T = ftrot_slip[i].T;
	#if !EN_TIME_CAL_ALAWYS1
					gait_pattern_controlf(&ftrot_slip_all);
	#endif 
					//next leg
					id_next = ftrot_slip[i].other_phase;//下一条腿
					ftrot_slip[id_next].T_st_now_trig = ftrot_slip_all.T_st_now;//触发更新支撑摆动时间
					ftrot_slip[id_next].T_tirg = ftrot_slip[id_next].T_sw + ftrot_slip[id_next].T_st_now;
					//vmc_all.param.param_vmc_default.stance_time[1] = vmc_all.param.param_vmc_default.stance_time[0] = ftrot_slip[id_next].T_tirg;
				
#else
					//next leg
					id_next = ftrot_slip[i].other_phase;//下一条腿
	#if !EN_TIME_CAL_ALAWYS1
					gait_pattern_controlf(&ftrot_slip[i]);
	#endif 

					ftrot_slip[id_next].T_st_now  = ftrot_slip[i].T_st_now;
					ftrot_slip[id_next].T_tirg = ftrot_slip[i].T_sw + ftrot_slip[i].T_st_now;
#endif
			}
		}
		slip_greg[F] = ftrot_slip[F].ground;
		slip_greg[B] = ftrot_slip[B].ground;
#endif
	break;
	}

	//SLIP
#if 1
	slip_mode.slip_F[Xr] = 0;
	slip_mode.slip_F[Zr] = 0;

	if (ftrot_slip[F].state == 1&& ftrot_slip[B].state == 1) {
		slip_mode.slip_F[Zr] += ftrot_slip[F].Fz / 2 + ftrot_slip[B].Fz / 2;
	}
	else {
		if (ftrot_slip[F].state == 1) {
			slip_mode.slip_F[Xr] = 0;
			slip_mode.slip_F[Zr] += ftrot_slip[F].Fz;
		}
		if (ftrot_slip[B].state == 1) {
			slip_mode.slip_F[Xr] = 0;
			slip_mode.slip_F[Zr] += ftrot_slip[B].Fz;
		}
	}
#else
	if (ftrot_slip[F].state == 1 && ftrot_slip[B].state == 1) {
		slip_mode.slip_F[Xr] = 0;
		slip_mode.slip_F[Zr] = ftrot_slip[F].Fz/2+ ftrot_slip[B].Fz / 2;
	}else if (ftrot_slip[F].state == 1 ) {
		slip_mode.slip_F[Xr] = 0;
		slip_mode.slip_F[Zr] = ftrot_slip[F].Fz ;
	}else if ( ftrot_slip[B].state == 1) {
		slip_mode.slip_F[Xr] = 0;
		slip_mode.slip_F[Zr] = ftrot_slip[B].Fz ;
	}
#endif	
	//-------摆动控制
	for (i = 0; i < 4; i++) {
		if (vmc[i].param.trig_state >= 1 && vmc[i].param.trig_state <= 3)//�����ڶ� ʹ���ٶ���ɰڶ��켣����
			swing_spd_control(i, dt);

		if (vmc[i].ground)
			vmc[i].st_phase += dt;
		else
			vmc[i].st_phase = 0;
		//}
	}
}

for (i = 0; i < 10; i++)
	timer_task[i] += dt;
}


#else 
void  Gait_FTrot_Update_v1(float dt)//old good
{
	char trig_state_flag[4] = { 0,0,0,0 };
	static char init[4], init_lisence = 0, state, cnt_time_change, switch_flag;
	static float time_delay, yaw_force;
	static float timer_task[10];
	char i, sel;
	float test_epos_b[3], test_epos_leg[3];
	static END_POS end_pos_n[4], end_pos_n_use[4];//ĩ�˹̶���ȫ������ϵ�µ�λ��
	static float cnt_mode_can_switch = 0, mode_en_switch_flag = 0;
	static float att_test_use[3];
	char leg_sel_trig = 0;
	float att_off[2];
	static float phast_time = 0;
	static char leg_phase[2] = { 0 };
	static char leg_other[2] = { 0 };
	if (timer_task[2] > 0.02) {
		body_traj_planner(timer_task[2]);
		timer_task[2] = 0;
	}


		if (vmc_all.gait_mode == F_TROT)//----------------------�Խǲ�̬
		{
			switch (vmc_all.trot_state)
			{
			case 0:
					if (cnt_time_change++ > 2 || 1) {//WS
						vmc_all.gait_time[1] = vmc_all.gait_time[3];
						vmc_all.stance_time = vmc_all.stance_time_auto;
						vmc_all.delay_time[2] = vmc_all.delay_time[1];
						cnt_time_change = 0;
					}

					
					if (switch_flag)
					{
						leg_phase[0] = FL1; leg_phase[1] = BL2;
						leg_other[0] = FL2; leg_other[1] = BL1;
					}
					else
					{
						leg_phase[0] = FL2; leg_phase[1] = BL1;
						leg_other[0] = FL1; leg_other[1] = BL2;
					}

					trig_plan(leg_phase[0], dt);
					vmc[leg_phase[0]].param.trig_state = 1;
					trig_plan(leg_phase[1], dt);
					vmc[leg_phase[1]].param.trig_state = 1;


					phast_time = 0;
					vmc_all.trot_state++;
					slip_mode.slip_F[Xr] = slip_mode.slip_F[Yr] = slip_mode.slip_F[Zr] = 0;
					slip_mode.slip_timer = 0;
				break;
			case 1://lift up
				phast_time += dt;
				slip_mode.slip_timer += dt;
				if (switch_flag) {
					trig_state_flag[FL1] = trig_lift(FL1, dt);
					trig_state_flag[BL2] = trig_lift(BL2, dt);
					if (trig_state_flag[FL1] && trig_state_flag[BL2]) {
						vmc[FL1].param.trig_state = 2;
						vmc[BL2].param.trig_state = 2;
						vmc_all.trot_state++;
					}
				}
				else {
					trig_state_flag[FL2] = trig_lift(FL2, dt);
					trig_state_flag[BL1] = trig_lift(BL1, dt);
					if (trig_state_flag[FL2] && trig_state_flag[BL1]) {
						vmc[FL2].param.trig_state = 2;
						vmc[BL1].param.trig_state = 2;
						vmc_all.trot_state++;
					}
				}
				break;
			case 2://swing&td
				slip_mode.slip_timer += dt;
				phast_time += dt;
				if (switch_flag)//--------------------
				{
					switch (vmc[FL1].param.trig_state)
					{
					case 2:
						trig_state_flag[FL1] = trig_swing(FL1, dt);
						if (trig_state_flag[FL1] == 1)//�л�֧��
							vmc[FL1].param.trig_state = 3;
						if (trig_state_flag[FL1] == 2)//�л�TD
							vmc[FL1].param.trig_state = 4;
						break;
					case 3:
						trig_state_flag[FL1] = trig_td(FL1, dt);
						if (trig_state_flag[FL1] >= 1)//�л�֧��
							vmc[FL1].param.trig_state = 4;
						break;
					}

					switch (vmc[BL2].param.trig_state)
					{
					case 2:
						trig_state_flag[BL2] = trig_swing(BL2, dt);
						if (trig_state_flag[BL2] == 1)//�л�֧��
							vmc[BL2].param.trig_state = 3;
						if (trig_state_flag[BL2] == 2)//�л�TD
							vmc[BL2].param.trig_state = 4;
						break;
					case 3:
						trig_state_flag[BL2] = trig_td(BL2, dt);
						if (trig_state_flag[BL2] >= 1)//�л�֧��
							vmc[BL2].param.trig_state = 4;
						break;
					}
					//printf("%d\n", vmc[FL1].ground&&vmc[FL2].ground&&vmc[BL1].ground&&vmc[BL2].ground);
#if GROUND_AFTER_TRIG	
					if (vmc[FL1].param.trig_state == 4 && vmc[BL2].param.trig_state == 4
#if FIX_ST_TIME
						&&phast_time > vmc_all.stance_time*MIN_ST_TIME_RATE
#endif
						) {
#else
					if (vmc[FL1].ground&&vmc[BL2].ground
#if FIX_ST_TIME
						&&phast_time > vmc_all.stance_time*MIN_ST_TIME_RATE
#endif
						) {
#endif
	
						phast_time = 0;
						slip_mode.slip_F[Xr] = slip_mode.slip_F[Yr] = slip_mode.slip_F[Zr] = 0;
						slip_mode.slip_timer = 0;
						vmc[FL1].ground = vmc[BL2].ground = 1;
						stand_force_enable_flag[FL1] = stand_force_enable_flag[BL2] = 1;
						vmc[FL1].param.trig_state = vmc[BL2].param.trig_state = 0;

						if (vmc_all.param.stand_trot_switch_flag == 1) {//վ���л�Trot
							vmc_all.param.stand_switch_cnt[0]++;
							vmc_all.param.trot_sw_end_flag[FL1] = vmc_all.param.trot_sw_end_flag[BL2] = 1;
							if (vmc_all.param.stand_switch_cnt[0] >= 1)//�л����
							{
								vmc_all.param.stand_switch_cnt[0] = 0;
								vmc_all.param.stand_switch_flag[1] = 0;//֧������
								vmc_all.param.stand_trot_switch_flag = 0;
							}
						}

						if (vmc_all.param.stand_trot_switch_flag == 2) {//Tort�л�վ��
							vmc_all.param.stand_switch_cnt[1]++;
							switch (vmc_all.param.stand_switch_cnt[1])
							{
							case 1:
								vmc_all.param.stand_switch_flag[1] = 1;//֧������ 
								break;
							case 3://�л����
								vmc_all.param.stand_switch_cnt[1] = 0;
								vmc_all.param.stand_trot_switch_flag = 0;
								break;
							}
						}

						if (vmc_all.delay_time[2] > 0)
							vmc_all.trot_state++;
						else
						{
							time_delay = vmc_all.trot_state = 0; switch_flag = !switch_flag;

							if (cnt_time_change++ > 2 || 1) {//WS
								vmc_all.gait_time[1] = vmc_all.gait_time[3];
								vmc_all.stance_time = vmc_all.stance_time_auto;
								vmc_all.delay_time[2] = vmc_all.delay_time[1];
								cnt_time_change = 0;
							}

							trig_plan(FL2, dt);//���ĵ����ڹ滮 �ֽ⵽����
							vmc[FL2].param.trig_state = 1;
							trig_plan(BL1, dt);
							vmc[BL1].param.trig_state = 1;
							vmc_all.trot_state = 1;
	
						}

					}
					}
				else//----------------------�Բ���
				{
					switch (vmc[FL2].param.trig_state)
					{
					case 2:
						trig_state_flag[FL2] = trig_swing(FL2, dt);
						if (trig_state_flag[FL2] == 1)//�л�֧��
							vmc[FL2].param.trig_state = 3;
						if (trig_state_flag[FL2] == 2)//�л�TD
							vmc[FL2].param.trig_state = 4;
						break;
					case 3:
						trig_state_flag[FL2] = trig_td(FL2, dt);
						if (trig_state_flag[FL2] >= 1)//�л�֧��
							vmc[FL2].param.trig_state = 4;
						break;
					}

					switch (vmc[BL1].param.trig_state)
					{
					case 2:
						trig_state_flag[BL1] = trig_swing(BL1, dt);
						if (trig_state_flag[BL1] == 1)//�л�֧��
							vmc[BL1].param.trig_state = 3;
						if (trig_state_flag[BL1] == 2)//�л�TD
							vmc[BL1].param.trig_state = 4;
						break;
					case 3:
						trig_state_flag[BL1] = trig_td(BL1, dt);
						if (trig_state_flag[BL1] >= 1)//�л�֧��
							vmc[BL1].param.trig_state = 4;
						break;
					}
					//printf("%d\n", vmc[FL1].ground&&vmc[FL2].ground&&vmc[BL1].ground&&vmc[BL2].ground);
#if GROUND_AFTER_TRIG
					if (vmc[FL2].param.trig_state == 4 && vmc[BL1].param.trig_state == 4
#if FIX_ST_TIME
						&&phast_time > vmc_all.stance_time*MIN_ST_TIME_RATE
#endif
						) {
#else
					if (vmc[FL2].ground&&vmc[FL1].ground
#if FIX_ST_TIME
						&&phast_time > vmc_all.stance_time*MIN_ST_TIME_RATE
#endif
						) {
#endif
						phast_time = 0;
					
						slip_mode.slip_F[Xr] = slip_mode.slip_F[Yr] = slip_mode.slip_F[Zr] = 0;
						slip_mode.slip_timer = 0;
						vmc[FL2].ground = vmc[BL1].ground = 1;
						stand_force_enable_flag[FL2] = stand_force_enable_flag[BL1] = 1;
						vmc[FL2].param.trig_state = vmc[BL1].param.trig_state = 0;

						if (vmc_all.param.stand_trot_switch_flag == 1) {//վ���л�Trot
							vmc_all.param.trot_sw_end_flag[FL2] = vmc_all.param.trot_sw_end_flag[BL1] = 1;
							vmc_all.param.stand_switch_cnt[0]++;
							if (vmc_all.param.stand_switch_cnt[0] >= 1)//�л����
							{
								vmc_all.param.stand_switch_cnt[0] = 0;
								vmc_all.param.stand_switch_flag[1] = 0;//֧������
								vmc_all.param.stand_trot_switch_flag = 0;
							}
						}

						if (vmc_all.param.stand_trot_switch_flag == 2) {//Tort�л�վ��
							vmc_all.param.stand_switch_cnt[1]++;
							switch (vmc_all.param.stand_switch_cnt[1])
							{
							case 1:
								vmc_all.param.stand_switch_flag[1] = 1;//֧������ 
								break;
							case 3://�л����
								vmc_all.param.stand_switch_cnt[1] = 0;
								vmc_all.param.stand_trot_switch_flag = 0;
								break;
							}
						}

						if (vmc_all.delay_time[2] > 0)
							vmc_all.trot_state++;
						else
						{
							time_delay = vmc_all.trot_state = 0; switch_flag = !switch_flag;

							if (cnt_time_change++ > 2 || 1) {//WS
								vmc_all.gait_time[1] = vmc_all.gait_time[3];
								vmc_all.stance_time = vmc_all.stance_time_auto;
								vmc_all.delay_time[2] = vmc_all.delay_time[1];
								cnt_time_change = 0;
							}

							trig_plan(FL1, dt);//���ĵ����ڹ滮 �ֽ⵽����
							vmc[FL1].param.trig_state = 1;
							trig_plan(BL2, dt);
							vmc[BL2].param.trig_state = 1;
							vmc_all.trot_state = 1;
 
						}
					}
					}
				break;
			case 4:
				time_delay += dt;
				if (time_delay > vmc_all.delay_time[2])
				{
					time_delay = vmc_all.trot_state = 0; switch_flag = !switch_flag;
				}
				break;
				}

			//SLIP

			if (slip_mode.slip_timer > 0) {

				slip_mode.slip_timer = limitw(slip_mode.slip_timer, 0, vmc_all.stance_time);
				slip_mode.slip_F[Xr] = -sind(slip_mode.slip_timer / vmc_all.stance_time * 180)*(vmc_all.tar_spd.x)*slip_mode.fx;
				slip_mode.slip_F[Zr] = sind(slip_mode.slip_timer / vmc_all.stance_time * 180)*(slip_mode.fz*0.2 + 0.8*fabs(vmc_all.tar_spd.x)*slip_mode.fz);
				if (fabs(vmc_all.tar_pos.z) < fabs(MAX_Z*0.6)) {
					//printf("ss\n");
					slip_mode.slip_F[Xr] = slip_mode.slip_F[Zr] = 0;
				}
				// printf("slip:tar_x=%f fx=%f fz=%f\n",vmc_all.tar_spd.x,slip_mode.slip_F[Xr],slip_mode.slip_F[Zr]);
			}
			//-------pos_force_control
			for (i = 0; i < 4; i++) {
				if (vmc[i].param.trig_state >= 1 && vmc[i].param.trig_state <= 3)//�����ڶ� ʹ���ٶ���ɰڶ��켣����
					swing_spd_control(i, dt);
				//}
			}
	}



	for (i = 0; i < 10; i++)
		timer_task[i] += dt;
}

#endif